package org.sunspotworld;

import L298NDriver.L298NDriver;
import com.sun.spot.io.j2me.radiogram.Radiogram;
import com.sun.spot.io.j2me.radiogram.RadiogramConnection;
import com.sun.spot.peripheral.ISleepManager;
import com.sun.spot.peripheral.Spot;
import com.sun.spot.peripheral.radio.IRadioPolicyManager;
import com.sun.spot.resources.transducers.IOutputPin;
import com.sun.spot.resources.transducers.ITriColorLED;
import com.sun.spot.resources.transducers.LEDColor;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.EDemoController;
import com.sun.spot.sensorboard.io.PinDescriptor;
import com.sun.spot.service.BootloaderListenerService;

import java.io.IOException;
import javax.microedition.io.Connector;

import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;

public class StartApplication extends MIDlet {

    private ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();

    private IOutputPin enableA = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H0];
    private IOutputPin inputA1 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D1];
    private IOutputPin inputA2 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D2];
    private IOutputPin enableB = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H1];
    private IOutputPin inputB1 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D3];
    private IOutputPin inputB2 = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D4];
    private IOutputPin logicSupply = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H3];
    private PinDescriptor enableADescriptor = EDemoController.H0;
    private PinDescriptor enableBDescriptor = EDemoController.H1;

    private ISleepManager sleep = Spot.getInstance().getSleepManager();

    private String port = "51";

    private int steerDir = 1;
    private int driveDir = 1;
    private int driveSpeed = 0;
    private int steerSpeed = 0;

    private boolean receiving = true;

    private static int FORWARD = 2;
    private static int LEFT = 2;
    private static int STILL = 1;
    private static int REVERSE = 0;
    private static int RIGHT = 0;

    protected void startApp() throws MIDletStateChangeException {

        new BootloaderListenerService().start();

        sleep.disableDeepSleep();

        L298NDriver drive = new L298NDriver(enableA,inputA1,inputA2, enableADescriptor);
        L298NDriver steer = new L298NDriver(enableB,inputB1,inputB2, enableBDescriptor);

        logicSupply.setHigh();

        IRadioPolicyManager rpm = Spot.getInstance().getRadioPolicyManager();
        rpm.setChannelNumber(25);
        rpm.setOutputPower(0);

        RadiogramConnection dgConnection = null;
        Radiogram dg = null;
        try {
            dgConnection = (RadiogramConnection) Connector.open("radiogram://:"+ port);
            dg = (Radiogram)dgConnection.newDatagram(dgConnection.getMaximumLength());
        } catch(IOException e) {

        }

        while(receiving) {
            try {
                dg.reset();
                dgConnection.receive(dg);
                driveSpeed = dg.readInt();
                driveDir = dg.readInt();
                steerDir = dg.readInt();
                steerSpeed = dg.readInt();
                receiving = true;
            } catch(IOException e) {

            }

            if(driveDir == FORWARD) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.GREEN);
                }

                Forward(drive,driveSpeed);
            }
            else if(driveDir == STILL) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.YELLOW);
                }

                FreeRunning(drive);
            }
            else if(driveDir == REVERSE) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setColor(LEDColor.RED);
                }

                Reverse(drive,driveSpeed);
            }

            if(steerDir == LEFT) {

                for(int i = 0; i < (leds.length - 4); i++) {
                    leds[i].setOn();
                }

                Left(steer,steerSpeed);
            }
            else if(steerDir == STILL) {

                for(int i = 0; i < leds.length; i++) {
                    leds[i].setOn();
                }

                FreeRunning(steer);
            }
            else if(steerDir == RIGHT) {

                for(int i = 4; i < leds.length; i++) {
                    leds[i].setOn();
                }

                Right(steer,steerSpeed);
            }
        }

        logicSupply.setLow();

        notifyDestroyed();                      // cause the MIDlet to exit
    }

    public void Forward(L298NDriver drive, int duty) {
        if(duty > 255)
            drive.DirectionA();
        else
            drive.PWMDirectionA(duty);
    }

    public void Reverse(L298NDriver drive, int duty) {
        if(duty > 255)
            drive.DirectionB();
        else
            drive.PWMDirectionB(duty);
    }

    public void Left(L298NDriver steer, int duty) {
        FreeRunning(steer);
        if(duty > 255)
            steer.DirectionB();
        else
            steer.PWMDirectionB(duty);
    }

    public void Right(L298NDriver steer, int duty) {
        FreeRunning(steer);
        if(duty > 255)
            steer.DirectionA();
        else
            steer.PWMDirectionA(duty);
    }

    public void FreeRunning(L298NDriver motor) {
        motor.FreeRunning();
    }

    public void Stop(L298NDriver motor) {
        motor.Stop();
    }

    protected void pauseApp() {
        // This is not currently called by the Squawk VM
    }

    /**
     * Called if the MIDlet is terminated by the system.
     * I.e. if startApp throws any exception other than MIDletStateChangeException,
     * if the isolate running the MIDlet is killed with Isolate.exit(), or
     * if VM.stopVM() is called.
     *
     * It is not called if MIDlet.notifyDestroyed() was called.
     *
     * @param unconditional If true when this method is called, the MIDlet must
     *    cleanup and release all resources. If false the MIDlet may throw
     *    MIDletStateChangeException  to indicate it does not want to be destroyed
     *    at this time.
     */
    protected void destroyApp(boolean unconditional) throws MIDletStateChangeException {
        for (int i = 0; i < 8; i++) {
            leds[i].setOff();
        }
    }
}
